#include "main.hpp"
PID_Controller x_pid_ctl;
PID_Controller y_pid_ctl;
PID_Controller z_pid_ctl;
PID_Controller yaw_pid_ctl;
rcPara_t x_filter;
rcPara_t y_filter;
rcPara_t z_filter;
int16_t desired_vel_x;
int16_t desired_vel_y;
int16_t desired_vel_z;
int16_t desired_yaw_rate;

int8_t autocontrol_flag = 0x00;

target_latlng reference_latlng; //设置参考经纬度
int8_t sampe_reference_number = 20;//参考点采样数
int16_t counter = 0;

uint8_t target_set_state;

uint8_t mession_state = 0x00;
uint8_t electromagnet_state = 0x00;
uint8_t son_plane_state = 0x00;


void NX_send_all_waypoints_to_flight_control()
{
    waypoinit_t all_waypoints[]={   //point_mode,point_duration,lat,lng,alt
                                {2,1,1,1,1},
                                {2,1,2,2,2},
                                {2,1,3,3,3}
                            };

    groundbase_waypoint.point_number = sizeof(all_waypoints)/sizeof(waypoinit_t);
    for(int i=0;i<groundbase_waypoint.point_number;i++)
    {
        groundbase_waypoint.current_index = i-1;
        groundbase_waypoint.next_index = i;
        groundbase_waypoint.point_mode = all_waypoints[i].point_mode;
        groundbase_waypoint.point_lat = all_waypoints[i].lat;
        groundbase_waypoint.point_lng = all_waypoints[i].lng;
        groundbase_waypoint.point_alt = all_waypoints[i].alt;
        groundbase_waypoint.point_duration = all_waypoints[i].point_duration;
        while(usr_flag.flight_point_flag==false);//等待飞控航点回传包校验飞控是否已经接收到航点
        usr_flag.flight_point_flag = false;
    }

}

void PID_Init(PID_Controller *PID, float kp, float ki, float kd, float output_limit, float integral_separate, float deadzone)
{
    PID->ErrorLimit_Flag = 1;
    PID->IntegralLimit_Flag = 0;
    PID->IntegralSeparation_Flag = 1;
    PID->Error = 0;
    PID->LastError = 0;
    PID->Error_Max = 0;
    PID->IntegralSeparation_Error = integral_separate;
    PID->Integrate = 0;
    PID->Integrate_Max = 10;
    PID->Kp = kp;
    PID->Ki = ki;
    PID->Kd = kd;
    PID->Output = 0;
    PID->Output_Limit = output_limit;
    PID->Output_deadzone = deadzone;
}
    /* 3阶Butterworth滤波 */
float Butterworth_Filter(float curr_inputer, ButterworthFilter_BufferData *Buffer, ButterworthFilter_Parameter *Parameter)
{

    /* 获取最新x(n) */
    Buffer->Input_Butter[2] = curr_inputer;
    /* Butterworth滤波 */
    Buffer->Output_Butter[2] =
    Parameter->b[0] * Buffer->Input_Butter[2] + Parameter->b[1] * Buffer->Input_Butter[1] + Parameter->b[2] * Buffer->Input_Butter[0] - Parameter->a[1] * Buffer->Output_Butter[1] - Parameter->a[2] * Buffer->Output_Butter[0];
    /* x(n) 序列保存 */
    Buffer->Input_Butter[0] = Buffer->Input_Butter[1];
    Buffer->Input_Butter[1] = Buffer->Input_Butter[2];
    /* y(n) 序列保存 */
    Buffer->Output_Butter[0] = Buffer->Output_Butter[1];
    Buffer->Output_Butter[1] = Buffer->Output_Butter[2];
    return (Buffer->Output_Butter[2]);
}
/* 低通滤波 */
float rcLfFiter(rcPara_t *rcPara, float val)
{
    rcPara->lVal=((float)val*rcPara->k+rcPara->lVal*(1-rcPara->k));
    return rcPara->lVal;
}

float PID_Control(PID_Controller *PID, float desire_value, float actual_value)
{
  // float Loop_dt = 0;
  // Loop_dt = PID->Loop_Time/1000.0;//获取控制步长

  //计算误差
  PID->LastError = PID->Error;                     //保存上次误差
  PID->Error = desire_value - actual_value;        //计算当前误差：期望-反馈
  PID->Differential = PID->Error - PID->LastError; //计算微分
    //PID->LastError_LPF = PID->Error_LPF;//保存上次滤波后的误差
    //PID->Error_LPF = Butterworth_Filter(PID->Error,&PID->ButterworthFilter_Buffer,&Vision_tracking_LPF_Parameter);//巴特沃斯低通后得到的微分项，20HZ
  //PID->Differential_LPF = PID->Error_LPF - PID->LastError_LPF;//误差经过低通后的微分量
  // if(PID->ErrorLimit_Flag == 1)//误差限幅
  // {
  // 	if(PID->Error_LPF >=  PID->Error_Max)  PID->Error_LPF =  PID->Error_Max;
  // 	if(PID->Error_LPF <= -PID->Error_Max)  PID->Error_LPF = -PID->Error_Max;
  // }
  // //计算积分量
  // if(PID->IntegralSeparation_Flag == 1)//积分分离
  // {
    // if(std::abs(PID->Error) <= PID->IntegralSeparation_Error)
    // {
    //     PID->Integrate += PID->Error;
    // }
    // else
    // {
    //     PID->Integrate = 0;
    // }
    PID->Integrate = std::abs(PID->Error) <= PID->IntegralSeparation_Error ? PID->Integrate+PID->Error : 0;
  		
  // }
  // else
  // {
  // 		PID->Integrate += PID->Error_LPF * Loop_dt;
  // }

  // if(PID->IntegralLimit_Flag == 1)//积分限幅
  // {
  // 	if(PID->Integrate >= PID->Integrate_Max)
  // 		PID->Integrate = PID->Integrate_Max;
  // 	if(PID->Integrate <= -PID->Integrate_Max)
  // 		PID->Integrate = -PID->Integrate_Max;
  // }

  //计算输出控制量
//   PID->Output = PID->Kp * PID->Error_LPF + PID->Ki * PID->Integrate + PID->Kd * PID->Differential;//比例+积分+微分
  PID->Output = PID->Kp * PID->Error + PID->Kd * PID->Differential + PID->Ki * PID->Integrate; //比例+积分+微分

    if (PID->Output >= PID->Output_Limit) //输出限幅
        PID->Output = PID->Output_Limit;
    if (PID->Output <= -PID->Output_Limit)
        PID->Output = -PID->Output_Limit;
    if (std::abs(PID->Error)<PID->Output_deadzone)
    {
        PID->Output = 0;
        PID->Integrate = 0;
        PID->Differential = 0;
    }
       
    

  return PID->Output; //返回输出量
}
void IARC_task_init()
{
    PID_Init(&x_pid_ctl, 0.41, 0.0018, 0, 10.0, 0.5, 0.02);//位置环x方向pid
    PID_Init(&y_pid_ctl, 0.41, 0.0018, 0, 10.0, 0.5, 0.02);//位置环y方向pid
    PID_Init(&z_pid_ctl, 0.7, 0.001, 0, 10.0, 0.1, 0.03);//位置环z方向pid
    PID_Init(&yaw_pid_ctl, 4.0, 0.01, 0, 10.0, 10.0, 1);//位置环偏航角pid

    x_filter.k = 0.9;
    y_filter.k = 0.9;
    z_filter.k = 0.85;
    //  printf("kp = %f, ki = %f , kd = %f \n",pid_x.Kp,pid_x.Ki ,pid_x.Kd);
    desired_vel_x = 0; // body vel    
    desired_vel_y = 0;
    desired_vel_z = 0;
    desired_yaw_rate = 0; //  0.0
    target_set_state = 1;

    printf("onboard init completed\n");
}
/***精确位置控制
 * x:X方向移动距离
 ** y:Y方向移动距离
 * *z:Z方向移动距离
 * * yaw：偏航线角
 * start_lat,start_lng,height:坐标原点经纬度高度
 * */
void position_ctrl_byvelocity(float x, float y, float z,float yaw,target_latlng reference_point)
{
    target_pos_t caculate_pos;//计算两个gps点间距离
    double  current_longitude = flight_control_to_NX_data.i_lon + 0.000000001*flight_control_to_NX_data.f_lon;
    double  current_latitude = flight_control_to_NX_data.i_lat + 0.000000001*flight_control_to_NX_data.f_lat;
    caculate_pos = get_vector_xy_from_current_position_NE_double(reference_point.target_lng,current_longitude,
                                                                    reference_point.target_lat,current_latitude);//通过rtk计算距离误差
    float psi = flight_control_to_NX_data.psi*0.01;
    psi = psi>180 ? psi - 360 : psi;
    //一阶低通滤波器
    float x_filted = rcLfFiter(&x_filter, caculate_pos.target_pos_x);
    float y_filted = rcLfFiter(&y_filter, caculate_pos.target_pos_y);
    float z_filted = rcLfFiter(&z_filter, flight_control_to_NX_data.alt*0.001-reference_point.height);
    //计算控制量
    desired_yaw_rate = PID_Control(&yaw_pid_ctl, yaw, psi)*16;                                                                                                                                            
    desired_vel_x = PID_Control(&x_pid_ctl, x, -x_filted)*84;
    desired_vel_y = PID_Control(&y_pid_ctl, y, y_filted)*84;
    desired_vel_z = PID_Control(&z_pid_ctl, z, z_filted)*280;

    ofstream fout;
    fout.open("/home/pi/4.25/onboard_project/test2.txt",ios::app);
    fout<<yaw_pid_ctl.Error<<","<<x_pid_ctl.Error<<","<<y_pid_ctl.Error<<","<<z_pid_ctl.Error<<endl;
    printf("yaw error:%f,x error:%f,y error:%f,z error:%f\n",yaw_pid_ctl.Error,x_pid_ctl.Error,y_pid_ctl.Error,z_pid_ctl.Error);
    printf("x_filted:%f\n",x_filted);
    printf("yaw:%d,x:%d,y:%d,z:%d\n",desired_yaw_rate,desired_vel_x,desired_vel_y,desired_vel_z);

    if (((std::abs(x_pid_ctl.Error)) < 0.02) && 
            ((std::abs(y_pid_ctl.Error)) < 0.02) &&
                (std::abs(z_pid_ctl.Error) <0.03)    &&
                    (std::abs(yaw_pid_ctl.Error)<3)) //0.1米偏差
    {   
        counter++;
        if(counter>5)
        {
            printf("compelete target_set_state:%d\n",target_set_state);
            target_set_state++;//进入下一步
            counter = 0;
        }
            
    }
}
 /***采参考点
 **/
void sampe_reference(target_latlng *ref_latlng)
{
    if(counter < sampe_reference_number)//采十个点取平均值
    {
        ref_latlng->target_lat += flight_control_to_NX_data.i_lat + 0.000000001*flight_control_to_NX_data.f_lat;
        ref_latlng->target_lng += flight_control_to_NX_data.i_lon + 0.000000001*flight_control_to_NX_data.f_lon;
        ref_latlng->height += flight_control_to_NX_data.alt*0.001;
        counter++;
    }
    else
    {
        counter = 0;
        ref_latlng->target_lat /= sampe_reference_number;//采十个点取平均值
        ref_latlng->target_lng /= sampe_reference_number;
        ref_latlng->height /= sampe_reference_number;//采十个点取平均值
        printf("x:%f,y:%f,z:%f\n", ref_latlng->target_lat,ref_latlng->target_lng,ref_latlng->height);
        printf("compelete target_set_state:%d\n",target_set_state);
        target_set_state++;
    }
}

void IARC_task(groundbase_cmd_t* para)
{
    switch (target_set_state)
        {
            case 1:
            {
                NX_send_all_waypoints_to_flight_control();
                target_set_state++;
                break;
            }
            case 2://基准点获取
            {
                sampe_reference(&reference_latlng);
                break;//！！！！！！！！！！！！注意
            }
            case  3://解锁
            {
                autocontrol_flag =0x01; 
                printf("compelete target_set_state:%d\n",target_set_state);
                target_set_state++;
                break;
            }
            case  4://    等待2s
            {
                if (counter<40)
                {
                    counter++;
                    
                }
                else
                {
                    printf("compelete target_set_state:%d\n",target_set_state);
                    target_set_state++;
                    counter = 0;
                }
                 break;//！！！！！！！！！！！！注意
            }
            case 5://上飞3m
            {
                autocontrol_flag =0x02;
                if(flight_control_to_NX_data.alt>4000)
                {
                    printf("compelete target_set_state:%d\n",target_set_state);
                    target_set_state++;
                }
                
                break;//！！！！！！！！！！！！注意
            }
            case 6:
            {
                autocontrol_flag =0x03;
                position_ctrl_byvelocity(0 ,0 , 3.0 ,0 ,reference_latlng);
                break;               
            }
            case 7:
            {
                autocontrol_flag =0x03;
                position_ctrl_byvelocity(0 ,2.0 , 3.0 ,0 ,reference_latlng);
                break;//！！！！！！！！！！！！注意
            }
            case 8:
            {
                autocontrol_flag =0x04;
                break;//！！！！！！！！！！！！注意
            }
        default:
        break;
        }
}